using Godot;
using System;
using System.Buffers.Text;
using System.Net.Sockets;
using System.Net.WebSockets;
using System.Text;
using System.Text.Unicode;
using System.Threading;
using System.Threading.Tasks;
using Newtonsoft.Json;
using System.Collections.Generic;
using System.Linq;

public partial class initialize : Node
{
	[Signal]
	public delegate void SignalSendRobotParametersEventHandler(string robotParameters);
	public WebSocketPeer clientWebsocket { get; set; }

	/// <summary>
	/// 轨迹追踪
	/// </summary>
	public MeshInstance3D nodeTrack { get; set; }

	public List<Vector3> listPoints { get; set; }

	/// <summary>
	/// 机械臂参数
	/// </summary>
	public RobotJoints robotJoints { get; set; }
	// Called when the node enters the scene tree for the first time.
	public override void _Ready()
	{
		clientWebsocket = new WebSocketPeer();
		ConnectServer();
		SignalSendRobotParameters += SendRobotParameters;

		nodeTrack = GetNode("Track") as MeshInstance3D;
		listPoints = new List<Vector3>();

		//连接信号
		for (int i = 0; i < 6; i++)
		{
			var node = GetNode("Robot").FindChild($"J{i + 1}");
			node.Connect("SignalUpdateSendRobotParameters", new Callable(this, "DataReceived"));
		}
	}

	/// <summary>
	/// 传递机械臂参数
	/// </summary>
	/// <param name="robotParameters">机械臂参数</param>
	private void SendRobotParameters(string robotParameters)
	{
		//GD.Print(JsonConvert.SerializeObject(robotJoints));
	}

	public override void _ExitTree()
	{
		clientWebsocket.Close();
	}

	public void DataReceived(string recievedData)
	{
		//GD.Print("recievedData");
		robotJoints = JsonConvert.DeserializeObject<RobotJoints>(recievedData);
	}

	/// <summary>
	/// 连接服务器
	/// </summary>
	private void ConnectServer()
	{
		clientWebsocket = new WebSocketPeer();
		var err = clientWebsocket.ConnectToUrl("ws://127.0.0.1:7080");
		if (err != Error.Ok)
		{
			SetProcess(false);
		}
		else
		{
			SetProcess(true);
			GD.Print("连接成功!");
			robotJoints = new RobotJoints();
			//InitializeRobotParameters();
		}
	}
	// Called every frame. 'delta' is the elapsed time since the previous frame.
	public override void _Process(double delta)
	{
		clientWebsocket.Poll();
		//clientWebsocket.Poll();
		clientWebsocket.SendText(JsonConvert.SerializeObject(robotJoints));
		WebSocketPeer.State state = clientWebsocket.GetReadyState();

		if (state == WebSocketPeer.State.Open)
		{
			if (clientWebsocket.GetAvailablePacketCount() > 0)
			{
				string data = clientWebsocket.GetPacket().GetStringFromUtf8();//将byte[]转成string
				robotJoints = JsonConvert.DeserializeObject<RobotJoints>(data);
				EmitSignal(SignalName.SignalSendRobotParameters, JsonConvert.SerializeObject(robotJoints));//传递更新信号
				DisplayTrack();
				//GD.Print(data);
			}
		}
		else if (state == WebSocketPeer.State.Closed)
		{
			int code = clientWebsocket.GetCloseCode();
			string reason = clientWebsocket.GetCloseReason();
			GD.Print("Disconnected from server:   code:", code, "   reason:", reason);
			SetProcess(false);
		}
	}

	/// <summary>
	/// 绘制轨迹
	/// </summary>
	public void DisplayTrack()
	{
		/*if (listPoints.Count ==0)
		{
			var point = robotJoints.endEffector;
			listPoints.Add(new Vector3(-(float)point.X/1000,(float)point.Y/1000,-(float)point.Z/1000));
		}else{
			var lastValue = listPoints.Last();
			var point = robotJoints.endEffector;
			if (lastValue[0]!=point.X&&lastValue[1]!=point.Y&&lastValue[2]!=point.Z)
			{
				listPoints.Add(new Vector3(-(float)point.X/1000,(float)point.Y/1000,-(float)point.Z/1000));
			}
		}
 	
		var arrays = new Godot.Collections.Array();
		arrays.Resize((int)Mesh.ArrayType.Max);
		arrays[(int)Mesh.ArrayType.Vertex] = listPoints.ToArray();
		ArrayMesh arrayMesh = new ArrayMesh();
		arrayMesh.AddSurfaceFromArrays(Mesh.PrimitiveType.Points, arrays);
		nodeTrack.Mesh = arrayMesh;*/
	}

	/// <summary>
	/// 初始化机械臂参数
	/// </summary>
	private void InitializeRobotParameters()
	{
		robotJoints = new RobotJoints();
		robotJoints.J1Angle = 0.0;
		robotJoints.J2Angle = 0.0;//90.0
		robotJoints.J3Angle = 0.0;
		robotJoints.J4Angle = 0.0;//-90.0
		robotJoints.J5Angle = 0.0;
		robotJoints.J6Angle = 0.0;

		/*SettingInfo settingInfo = new SettingInfo();
		settingInfo.isCleanTrack = false;
		settingInfo.isTrackOpen = true;
		robotJoints.settingInfo = settingInfo;

		EndEffector endEffector = new EndEffector();
		endEffector.X = 0.0d;
		endEffector.Y = 0.0d;
		endEffector.Z = 0.0d;
		robotJoints.endEffector = endEffector;*/
	}
}
